/******************************************************************************
 * 用Stanley算法实现横向轨迹跟踪，用PID实现纵向速度控制.
 * @author:HuYunhao
 * @Ref:
 *  - [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf)
    - [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)
 ******************************************************************************/

#ifndef CPP_STANLEY_CONTROLLER_H
#define CPP_STANLEY_CONTROLLER_H

#include <iostream>
#include <fstream>
#include "my_functions.h"
#include "nlohmann/json.hpp"

namespace controlNS {
using namespace std;

// 读取控制参数
extern nlohmann::json control_json;     //存放控制参数的json配置文件
const float kPI = myNumpy::kPI;         //定义pi
extern const float kStanleyGain;        //stanley控制增益参数
extern const float kSpeedPGain;         //速度P控制器增益
extern const float kTimeDifference;     //仿真时间间隔[s]
extern  const float kWheelBase;         //车辆轴距
extern const float kMaxSteerAngle;      //最大车辆转角[rad]
extern const float kSteeringRatio;      //车辆的传动比

class State {
    /**用来表示车辆状态的类*/
public:
    //初始化车辆的状态
    explicit State(float x = 0, float y = 0, float yaw = 0, float v = 0);

    /**更新车辆的状态
     * @param acceleration 车辆的加速度[m/s^2]
     * @param delta 方向盘的转角[rad]
     * @return void
     */
    void Update(float acceleration, float delta);
    void UpdateForPreScan(float x, float y, float yaw, float v);
public:
    float x_;   //车辆的全局x坐标[m]
    float y_;   //车辆的全局y坐标[m]
    float yaw_; //车辆的横摆角[rad]
    float v_;   //车辆的全局速度[m/s]

};

class StanleyController {
    /**基于Stanley理论的横向控制器*/
public:
    StanleyController();
    ~StanleyController();
    void calcPath(const State& state);    //计算车辆前轮中心的坐标[fx_, fy_];
    void calcNearestIndex();
    void calcError();
    float calcDelta(const State& state);

public:
    float fx_;                  //车辆前轮中心的x坐标[m]
    float fy_;                  //车辆前轮中心的y坐标[m]
    float yaw_;                 //车辆的yaw angle[rad]
    int min_distance_index_;    //参考点的索引
    float cross_track_error_;   //横向偏差[m]
    float heading_error_;       //航向偏差[rad]
    vector<float> cx_;          //规划路径的x坐标[m];
    vector<float> cy_;          //规划路径的y坐标[m];
    vector<float> cyaw_;        //规划路径的切线角[rad];
    float delta_;               //stanley算法计算出的车轮转角[rad]


};

/**用比例控制来控制速度
 * @parm target_velocity [m/s]
 * @parm current_velocity [m/s]
 * @return 控制指令 加速度[m/s^2]
 */
float PidControl(float target_velocity, float current_velocity);

};  //namespace controlNS
#endif //CPP_STANLEY_CONTROLLER_H
